#include<sys/socket.h>
#include<arpa/inet.h>
#include<netinet/in.h>
#include<stdio.h>
#include<string.h>
#include<stdlib.h>
#include<unistd.h>
#include<pthread.h>
#include<ctype.h>
#include<errno.h>

#include <core.h>

#define A(x) (struct sockaddr*)(&x)
 
int listenfd = 0, connfd = 0;
int n;
int i;
struct sockaddr_in serv_addr; 
time_t ticks; 

#define pinECHO		0
#define pinTRIG		1


#define pinI1     8  //define IN1 interface
#define pinI2     11 //define IN2 interface 
#define speedpinA 9  //enable motor A
#define pinI3     12 //define IN3 interface 
#define pinI4     13 //define IN4 interface 
#define speedpinB 10 //enable motor B
int spead1 = 50;//define the spead of motor
int spead2 = 50;
int led = 0;
char send_buf[10];
void forward()
{
     analogWrite(speedpinA,spead1);//input a simulation value to set the speed
     analogWrite(speedpinB,spead2);
     digitalWrite(pinI4,HIGH);//turn DC Motor B move clockwise
     digitalWrite(pinI3,LOW);
     digitalWrite(pinI2,LOW);//turn DC Motor A move anticlockwise
     digitalWrite(pinI1,HIGH);
     led = 0;
}
void backward()
{
     analogWrite(speedpinA,spead1);//input a simulation value to set the speed
     analogWrite(speedpinB,spead2);
     digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
     digitalWrite(pinI3,HIGH);
     digitalWrite(pinI2,HIGH);//turn DC Motor A move clockwise
     digitalWrite(pinI1,LOW);
	 led = 1;
}
void left()
{
     analogWrite(speedpinA,spead1);//input a simulation value to set the speed
     analogWrite(speedpinB,20);
     digitalWrite(pinI4,HIGH);//turn DC Motor B move clockwise
     digitalWrite(pinI3,LOW);
     digitalWrite(pinI2,HIGH);//turn DC Motor A move clockwise
     digitalWrite(pinI1,LOW);
     led = 2;
}
void right()
{
     analogWrite(speedpinA,20);//input a simulation value to set the speed
     analogWrite(speedpinB,spead2);
     digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
     digitalWrite(pinI3,HIGH);
     digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
     digitalWrite(pinI1,HIGH);
	 led = 3;
}
void stop()
{
	analogWrite(speedpinA,0);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
	analogWrite(speedpinB,0);
	digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
	digitalWrite(pinI3,LOW);
	digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
	digitalWrite(pinI1,LOW);
	led = 4;
}

void spead_up()
{
    if(spead1 < 250 && spead2 <250)
    {
	    spead1 += 10;
	    spead2 += 10;
    }
	analogWrite(speedpinA,spead1);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
    analogWrite(speedpinB,spead2);
    digitalWrite(pinI4,HIGH);//turn DC Motor B move anticlockwise
    digitalWrite(pinI3,LOW);
    digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
    digitalWrite(pinI1,HIGH);
	led = 5;
}

void spead_down()
{
    if(spead1 > 70 && spead2 > 70)
    {
	    spead1 -= 10;
	    spead2 -= 10;
    }
	analogWrite(speedpinA,spead1);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
    analogWrite(speedpinB,spead2);
    digitalWrite(pinI4,HIGH);//turn DC Motor B move anticlockwise
    digitalWrite(pinI3,LOW);
    digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
    digitalWrite(pinI1,HIGH);
	led = 6;	
}

void spead_lu()
{
    if(spead1 <250)
	    spead1 += 10;
	analogWrite(speedpinA,spead1);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
    analogWrite(speedpinB,spead2);
    digitalWrite(pinI4,HIGH);//turn DC Motor B move anticlockwise
    digitalWrite(pinI3,LOW);
    digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
    digitalWrite(pinI1,HIGH);
	led = 7;
}

void spead_ld()
{
    if(spead1 >100)
	    spead1 -= 10;
	digitalWrite(speedpinA,spead1);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
    digitalWrite(speedpinB,spead2);
    digitalWrite(pinI4,HIGH);//turn DC Motor B move anticlockwise
    digitalWrite(pinI3,LOW);
    digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
    digitalWrite(pinI1,HIGH);
	led = 8;
}

void spead_ru()
{
    if(spead2 < 250)
	    spead2 += 10;
	analogWrite(speedpinA,spead1);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
    analogWrite(speedpinB,spead2);
    digitalWrite(pinI4,HIGH);//turn DC Motor B move anticlockwise
    digitalWrite(pinI3,LOW);
    digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
    digitalWrite(pinI1,HIGH);
	led = 9;
}

void spead_rd()
{
    if(spead2 > 100)
    	spead2 -= 10;
	analogWrite(speedpinA,spead1);// Unenble the pin, to stop the motor. this          should be done to avid damaging the motor. 
    analogWrite(speedpinB,spead2);
    digitalWrite(pinI4,HIGH);//turn DC Motor B move anticlockwise
    digitalWrite(pinI3,LOW);
    digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
    digitalWrite(pinI1,HIGH);
	led = 10;
}

void port_init()
{
    pinMode(pinI1,OUTPUT);
    pinMode(pinI2,OUTPUT);
    pinMode(speedpinA,OUTPUT);
    pinMode(pinI3,OUTPUT);
    pinMode(pinI4,OUTPUT);
    pinMode(speedpinB,OUTPUT);
    for( i = 2; i < 8;i++)
    {
	    pinMode(i,OUTPUT);
    }
    delay(100);
}

void* func(void *arg)
{
    int c = *(int *)arg;
	char buf[10];
	while(1)
	{
	    int n = read(c,buf,sizeof(buf)-1);
	    if(n<=0) break;
	    buf[n] = '\0';
        write(1,buf,n);
    	switch(buf[0])
	    {
	        case 'g':        forward();break;
	        case 'b':        backward(); break;
	        case 'l':        left();break;
	        case 'r':        right();break;
	        case 'u':		spead_up(); break;
	        case 'd':		spead_down();break;
	        case 'i':        spead_lu(); break;
	        case 'k':        spead_ld();  break;
	        case 'j':        spead_ru(); break;
	        case 'm':        spead_rd; break;
	        case 's':		stop();break;
	    }
    }
}

int get_dis()
{
	digitalWrite(pinTRIG, LOW); // 保持超声波控制接口低电平2us
	delayMicroseconds(2);

	digitalWrite(pinTRIG, HIGH); // 保持超声波控制接口高电平10us
	delayMicroseconds(10);
	digitalWrite(pinTRIG, LOW);    // 超声波控制接口转低电平以触发测距过程

	// 距离测试公式为 脉冲长度 * 音速 / 2;
//	const int SONIC_SPEED = 340; // 音速在大气中传播速度为340米/秒
	int dis = pulseIn(pinECHO, HIGH,-1>>1); // 读出脉冲时间
	dis = dis * 170 / 10000;
	printf("duration:%d\n cm",dis);
	return dis;
}
void *dis(void *arg)
{
	while(1)
	{
		int s = get_dis();
		if(s <= 10)
		{
			if(s > 5)
				left();
			else
				backward();
		}
	}	
}

void led_off()
{
	for(i = 2; i < 8; i++)
	{
		digitalWrite(i, HIGH);
	}
}
void *led_ctrl(void *arg)
{
	while(1)
	{
		switch(led)
		{
			case 0:
				digitalWrite(6, LOW);
				digitalWrite(2, LOW);
				digitalWrite(5, LOW);
				delay(300);
				digitalWrite(2, HIGH);
				digitalWrite(6, HIGH);
				digitalWrite(5, HIGH);
				break;
			case 1:
				digitalWrite(6, LOW);
				digitalWrite(3, LOW);
				digitalWrite(4, LOW);
				delay(300);
				digitalWrite(3, HIGH);
				digitalWrite(6, HIGH);
				digitalWrite(4, HIGH);
				break;
			case 2:
				digitalWrite(6, LOW);
				digitalWrite(4, LOW);
				digitalWrite(5, LOW);
				delay(300);
				digitalWrite(6, HIGH);
				digitalWrite(4, HIGH);
				digitalWrite(5, HIGH);
				break;
			case 3:
				digitalWrite(6, LOW);
				digitalWrite(2, LOW);
				digitalWrite(3, LOW);
				delay(300);
				digitalWrite(2, HIGH);
				digitalWrite(6, HIGH);
				digitalWrite(3, HIGH);
				break;
			case 4:
				digitalWrite(6, LOW);
				digitalWrite(7, LOW);
				digitalWrite(2, LOW);
				digitalWrite(3, LOW);
				digitalWrite(4, LOW);
				digitalWrite(5, LOW);
				delay(300);
				digitalWrite(7, HIGH);
				digitalWrite(6, HIGH);
				digitalWrite(2, HIGH);
				digitalWrite(3, HIGH);
				digitalWrite(4, HIGH);
				digitalWrite(5, HIGH);
				break;
			case 5:
				digitalWrite(7, LOW);
				digitalWrite(3, LOW);
				digitalWrite(4, LOW);
				delay(300);
				digitalWrite(7, HIGH);
				digitalWrite(4, HIGH);
				digitalWrite(3, HIGH);
				break;
			case 6:
				digitalWrite(7, LOW);
				digitalWrite(2, LOW);
				digitalWrite(5, LOW);
				delay(300);
				digitalWrite(2, HIGH);
				digitalWrite(5, HIGH);
				digitalWrite(7, HIGH);
				break;
			case 7:
				digitalWrite(7, LOW);
				digitalWrite(5, LOW);
				delay(300);
				digitalWrite(7, HIGH);
				digitalWrite(5, HIGH);
				break;
			case 8:
				digitalWrite(7, LOW);
				digitalWrite(4, LOW);
				delay(300);
				digitalWrite(4, HIGH);
				digitalWrite(7, HIGH);
				break;	
			case 9:
				digitalWrite(7, LOW);
				digitalWrite(2, LOW);
				delay(300);
				digitalWrite(2, HIGH);
				digitalWrite(7, HIGH);
				break;
			case 10:
				digitalWrite(7, LOW);
				digitalWrite(3, LOW);
				delay(300);
				digitalWrite(3, HIGH);
				digitalWrite(7, HIGH);
				break;
		}
	}
}
int s;
struct sockaddr_in si; 
pthread_t tid1, tid2, tid3; 
void setup()
{    
	port_init();
	led_off();
	s = socket(AF_INET,SOCK_STREAM,0);		
	si.sin_family = AF_INET;
	si.sin_addr.s_addr =  INADDR_ANY;
	si.sin_port = htons(9999);
	if(bind(s,A(si),sizeof(si)))
	{
		printf("bind error"); 
		exit(1);
	}
    listen(s,5);
    printf("listening...\n");

//	pthread_create(&tid1,NULL,func,NULL);
//	pthread_detach(tid1);		
	pthread_create(&tid2,NULL,dis,NULL);
	pthread_detach(tid2);	
	pthread_create(&tid3,NULL,led_ctrl,NULL);
	pthread_detach(tid3);	
}


void loop()
{
        port_init();
    	socklen_t len = sizeof(si);
		int c = accept(s,A(si),&len);
		while(c<0);
		//welcome friend form ip,port
        char buf[100],ip[100];
		
        inet_ntop(si.sin_family,&si.sin_addr,ip,sizeof(ip));
        printf("connect success\n");
        while(1)
        {
        sprintf(send_buf, "left_spead: %d, right_spead: %d\n", spead1, spead2);

        send(c, send_buf, strlen(send_buf), 0);
        memset(send_buf, 0, sizeof(send_buf));
        delay(1000);
		pthread_create(&tid1,NULL,func,(void*)&c);
		pthread_detach(tid1);		
//        printf("send\n");
        }
}
